/******************************************************************
编程环境：----------
        软件:Arduino2.3.4
        硬件:Mega2560主控板, Mega2560扩展版,3s电池盒，锂电池*3
             超声波传感器*1, Windows11操作系统电脑，方口USB数据线*1
             四麦克纳姆轮小车
        库函数:<Arduino.h>, <uni_motor.h>

实验目的：-----------
        1.了解超声波传感器测距原理；
        2.搭建机器人自主避障框架。

操作流程：----------
        详细请看操作视频.

实验现象：-----------
        机器人在搭建的迷宫中实现自主避障.当小车的超声波检测到距离小于26厘米时，
        小车后退800毫秒，之后再左转2500毫秒，否则，小车前进.

实验接线简略图：----------
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
    电池仓         mega2560扩展版         超声波传感器        车轮
    12V母座~~~~~~~~~板载供电口1                
                      5V  -----------------3V3/5V
                      PA2(24)--------------Trig
                      PA0(22) -------------Echo
                      GND ----------------- GND
                      M1  ................................小车左前轮
                      M2  ................................小车右前轮
                      M3  ................................小车左后轮
                      M4  ................................小车右后轮
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
2024.12.19 Uni Tongchuang Intelligent Robot Technology CO.,Ltd
*******************************************************************/
#include "uni_motor.h" //调用编码电机库
#define RATE 115200    //定义串口波特率
#define motor_speed_lf  0.1 //设定小车左前轮速度,单位:m/s
#define motor_speed_rf  0.1 //设定小车右前轮速度,单位:m/s
#define motor_speed_lb  0.1 //设定小车左后轮速度,单位:m/s
#define motor_speed_rb  0.1 //设定小车右后轮速度,单位:m/s

//电机参数部分
/*枚举小车状态
  FORWARD:前进; BACK:后退; LEFT:左转
  RIGHT:右转; TRANS_LEFT:左平移; TRANS_RIGHT:右平移; STOP:停止 */
enum{FORWARD=1,BACK,LEFT,RIGHT,TRANS_LEFT,TRANS_RIGHT,STOP};
UNI_MOTOR motor1(1, true);  //声明小车左前轮
UNI_MOTOR motor2(2, true);  //声明小车右前轮
UNI_MOTOR motor3(3, true);  //声明小车左后轮
UNI_MOTOR motor4(4, true);  //声明小车右后轮

//超声波参数部分
int Trig = 24;              //超声波Trig引脚
int Echo = 22;              //超声波Echo引脚
float distance;             //定义存储超声波距离变量
int set_ultrasonic_dis = 26;//设定收到超声波实验值，单位:厘米

//程序初始化部分
void setup() {
  delay(2500);
  Serial.begin(RATE);      //打开串口
  motor_init();            //编码电机引脚初始化
  ultrasonic_init();       //超声波传感器引脚初始化
}

//程序主循环部分
void loop() {
  float get_ultrasonic = get_value(Trig, Echo); //获取超声波距离值
  //Serial.println(get_ultrasonic);    //串口打印读取到的超声波距离值
  if(get_ultrasonic < set_ultrasonic_dis){  //如果检测到超声波距离小于26厘米
    motor_move(BACK, 800);  //小车后退500毫秒
    motor_move(LEFT, 2500); //小车左转1000毫秒
  }
  else{ car_forward(); }             //否则，小车前进
  motor_update();//更新电机参数，维护电机pid程序，不可删除
}

